#ifndef ROSINTERFACE_H
#define ROSINTERFACE_H

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_srvs/Empty.h"
#include "turtlesim/Pose.h"
#include "turtlesim/Velocity.h"
#include <pthread.h>

//! Struct used for real time thread
struct thread_param{
	int prio;			// Thread priority level 0[min]-80[max]
	int clock;  		// CLOCK_MONOTONIC or CLOCK_REALTIME
};
//! struct to store main data for a thread
typedef struct thread_data{
    //! Priority and clock
    struct thread_param pthreadPar;
    //! Contains the desired frequency of the thread
    double dDesiredHz;
    //! Contains the real frequency of the thread
    double dRealHz;
    //! Contains the id of the control thread
    pthread_t pthreadId;
}thread_data;

//! Main function to execute into the thread
void *AuxControlThread(void *threadParam);

class RosInterface
{
    friend void *AuxControlThread(void *threadParam);
    public:
        RosInterface();
        virtual ~RosInterface();
        int Start();
        turtlesim::Pose GetTurtlePose();
    protected:

        virtual void turttleposeCallback(const turtlesim::Pose::ConstPtr& msg);
        virtual void ControlThread();
        ros::NodeHandle n;
        ros::Subscriber sub;    // Suscriber
        ros::Publisher pub;     // publisher
        ros::ServiceClient client_clear; // service to clear the screen
        std_srvs::Empty srv_clear;
        //! Contains data for the main thread
		thread_data threadData;
		turtlesim::Pose turtlePose; // Pose msg for the turtle
		turtlesim::Velocity turtleVelocity; // Velocity msg for the turtle

};

#endif // ROSINTERFACE_H
